#include "UR.h"
#include "timer.h"
#include "sys.h"

angle UR_angle;

void UR_control(int angle1,int angle2, int angle3,int angle4,int angle5)
{
	
	TIM_SetCompare1(TIM3,(200-(angle1+67.5)/13.5));
	delay_ms(100);
	TIM_SetCompare2(TIM3,200-(angle2+45)/9);
	delay_ms(100);
	TIM_SetCompare3(TIM3,(int)(200-(angle3+67.5)/13.5));
	delay_ms(100);
	TIM_SetCompare1(TIM5,200-(angle4+45)/9);
	delay_ms(100);
  TIM_SetCompare3(TIM5,200-(angle5+45)/9);		
	delay_ms(100);
}
void UR_SingleCon(int angle,int index)
{
	if(index<=5)
	{
  UR_angle.angle[index-1]=angle;
	UR_control(UR_angle.angle[0],UR_angle.angle[1],UR_angle.angle[2],UR_angle.angle[3],UR_angle.angle[4]);
	}
}

void UR_init(int Angle[])
{
  int i;
	for(i=0;i<5;i++)
	{
  UR_angle.angle[i]=Angle[i];
  }
}